/*****************************************************************************************//**
 * @file main.c
 * @author	Kirilly Miklos
 * 			Pinter Gyula
 * @date	2010-05-20
 ********************************************************************************************/
#include <cyg/hal/hal_platform_ints.h>
#include <cyg/hal/lpc2106_registers.h>
#include <cyg/kernel/kapi.h>
#include <cyg/hal/hal_diag.h>
#include <cyg/infra/diag.h>
#include <cyg/hal/hal_io.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <dpy_trm_s01.h>
#include <com_r04_s01b.h>

/********************************************************************************************/
// constant definitions
/********************************************************************************************/
#define TASK_STACK_SIZE	         1024
#define TASK_REQUEST_PRIO 11
#define TASK_RADIO_RECEIVE_PRIO 13
#define TASK_ANSWER_PRIO       12
#define CYGNUM_HAL_PRI_HIGH 0
#define CLK_SYNC_REQUEST 0x00
#define CLK_SYNC_ANSWER 0xFF
#define MACROTICK 800000
#define NUMBER_OF_NODES 16
#define MICROTICK (MACROTICK / NUMBER_OF_NODES - 200)
#define PACKET_SIZE sizeof(struct packet_t)
#define BLINK_PERIOD (1<<25)
#define ERROR_INIT_VALUE 1 << 30
#define CLK_PRECISION 30 // ppm

/********************************************************************************************/
// macros
/********************************************************************************************/
#define settime(val) HAL_WRITE_UINT32(TIMER1_TC, val);
#define setmr0(val) HAL_WRITE_UINT32(TIMER1_MR0, val); // request_timer - macrotick
#define setmr1(val) HAL_WRITE_UINT32(TIMER1_MR1, val); // wait - microtick
#define gettime() (*(volatile CYG_WORD32 *)(TIMER1_TC))
#define getmr0() (*(volatile CYG_WORD32 *)(TIMER1_MR0)) // request_timer - macrotick
#define getmr1() (*(volatile CYG_WORD32 *)(TIMER1_MR1)) // wait - microtick
/// message packet
struct packet_t {
	cyg_uint8 source; ///< message sender
	cyg_uint8 destination; ///< message addressee
	cyg_uint8 message_type; ///< message type, possible types are CLK_SYNC_REQUEST and CLK_SYNC_ANSWER
	cyg_uint32 Cj; ///< clock
	cyg_int32 Ej; ///< approximated clock error
};

/********************************************************************************************/
// function declaration
/********************************************************************************************/
void init_Timer1(void);
void get_msg(cyg_addrword_t index);
void timed_request(cyg_addrword_t index);
void radio_receive(cyg_addrword_t index);
void clk_sync_init(void);
void clk_sync_handle_request(struct packet_t*);
void clk_sync_handle_answer(struct packet_t*);
void timed_answer(cyg_addrword_t index);

/********************************************************************************************/
// variables
/********************************************************************************************/
// OS variables
int timed_request_stack[TASK_STACK_SIZE];
cyg_handle_t timed_request_handle;
cyg_thread timed_request_obj;

int radio_receive_stack[TASK_STACK_SIZE];
cyg_handle_t radio_receive_handle;
cyg_thread radio_receive_obj;

int timed_answer_stack[TASK_STACK_SIZE];
cyg_handle_t timed_answer_handle;
cyg_thread timed_answer_obj;

/// flag used to schedule synchronization messages
/// @see void timed_answer(cyg_addrword_t) and void timed_request(cyg_addrword_t)
cyg_flag_t my_flag;

/// mutex used to synchronize access to the RF chip
cyg_mutex_t my_mutex;

static cyg_interrupt timer1_int;
static cyg_handle_t timer1_int_handle;

/// Interrupt utility variable, stores the TIMER1_IR value for the DSR
/// after it is cleared in the ISR.
volatile cyg_uint32 which_int = 0;

// Clock synchronization global variables
cyg_int32 CLK_SYNC_h; ///< clock uncertainty at last synchronization time
cyg_uint32 CLK_SYNC_rho; ///< last synchronization time
float CLK_SYNC_drift; ///< approximated clock drift
cyg_uint32 CLK_SYNC_request_sent_time; ///< time of the sent sync request
cyg_uint8 OWN_ID; ///< mote identifier, set on the DIP switches of the display card

cyg_uint8 requester; ///<ID of the mote that requested sync

bool starting;

/*****************************************************************************************//**
 * timer 1 interrupt service routine
 *
 * Acknowledges interrupt and reads interrupt source into which_int.
 * Also re-initializes match registers.
 ********************************************************************************************/
cyg_uint32 Timer1_isr(cyg_vector_t vector, cyg_addrword_t data) {
	cyg_interrupt_mask(vector);
	cyg_interrupt_acknowledge(vector);
	cyg_uint32 oldreg;
	HAL_READ_UINT32(TIMER1_MCR, oldreg);

	HAL_READ_UINT32(TIMER1_IR, which_int);

	switch (which_int) {
	case 1: // request timer - macrotick
		HAL_WRITE_UINT32(TIMER1_IR, 1);
		oldreg = (oldreg & ~0x01);
		HAL_WRITE_UINT32(TIMER1_MCR, oldreg);
		break;
	case 2: //wait - microtick
		HAL_WRITE_UINT32(TIMER1_IR, 2);
		oldreg = (oldreg & ~0x08);
		HAL_WRITE_UINT32(TIMER1_MCR, oldreg);
		break;
	case 4: // LED blinker
		HAL_WRITE_UINT32(TIMER1_IR, 4);
		HAL_READ_UINT32(TIMER1_MR2, oldreg);
		HAL_WRITE_UINT32(TIMER1_MR2, oldreg+BLINK_PERIOD/2);

		break;
	case 5:
		// macrotick
		HAL_WRITE_UINT32(TIMER1_IR, 5);
		oldreg = (oldreg & ~0x01);
		HAL_WRITE_UINT32(TIMER1_MCR, oldreg);
		// LED
		HAL_READ_UINT32(TIMER1_MR2, oldreg);
		HAL_WRITE_UINT32(TIMER1_MR2, oldreg+BLINK_PERIOD/2);
		break;
	case 6:
		//wait - microtick
		HAL_WRITE_UINT32(TIMER1_IR, 6);
		oldreg = (oldreg & ~0x08);
		HAL_WRITE_UINT32(TIMER1_MCR, oldreg);
		// LED
		HAL_READ_UINT32(TIMER1_MR2, oldreg);
		HAL_WRITE_UINT32(TIMER1_MR2, oldreg+BLINK_PERIOD/2);
		break;
	default:
		HAL_WRITE_UINT32(TIMER1_IR, 7);
		oldreg = (oldreg & ~0x09);
		HAL_WRITE_UINT32(TIMER1_MCR, oldreg);
		// LED
		HAL_READ_UINT32(TIMER1_MR2, oldreg);
		HAL_WRITE_UINT32(TIMER1_MR2, oldreg+BLINK_PERIOD/2);
		break;
	}

	return (CYG_ISR_CALL_DSR);
	// The other option is (when there is no need to a DSR): CYG_ISR_HANDLED
}

/*****************************************************************************************//**
 * timer 1 deferred service routine
 *
 * Sets flag for request and answer threads.
 ********************************************************************************************/
void Timer1_dsr(cyg_vector_t vector, cyg_ucount32 count, cyg_addrword_t data) {

	switch (which_int) {
	// schedule next request; N*macroticks
	// void radio_request(cyg_addrword_t index)
	case 1:
		cyg_flag_setbits(&my_flag, 1);
		break;
		// schedule next answer - microtick
	case 2:
		cyg_flag_setbits(&my_flag, 2);
		break;
	case 4:
		if (DPY_TRM_S01__LED_1_GET_STATE()) {
			DPY_TRM_S01__LED_1_OFF();
		} else {
			DPY_TRM_S01__LED_1_ON();
		}
		break;
	case 5:
		cyg_flag_setbits(&my_flag, 1);
		if (DPY_TRM_S01__LED_1_GET_STATE()) {
			DPY_TRM_S01__LED_1_OFF();
		} else {
			DPY_TRM_S01__LED_1_ON();
		}
		break;
	case 6:
		cyg_flag_setbits(&my_flag, 2);
		if (DPY_TRM_S01__LED_1_GET_STATE()) {
			DPY_TRM_S01__LED_1_OFF();
		} else {
			DPY_TRM_S01__LED_1_ON();
		}
		break;
	default:
		cyg_flag_setbits(&my_flag, 1);
		cyg_flag_setbits(&my_flag, 2);
		if (DPY_TRM_S01__LED_1_GET_STATE()) {
			DPY_TRM_S01__LED_1_OFF();
		} else {
			DPY_TRM_S01__LED_1_ON();
		}
		break;
	}

	cyg_interrupt_unmask(vector);
}

/*****************************************************************************************//**
 * entry point, called in the startup procedure of eCos
 ********************************************************************************************/
void cyg_user_start(void) {

	diag_printf("Radio chat (%s -- %s)\r\n", __DATE__,__TIME__);
	// Initializing thread communication
			cyg_flag_init(&my_flag);
			cyg_mutex_init(&my_mutex); // initializing the mutex

			//API inits
			com_r04_s01__Init_433(SUB_BAND_434_2_MHz, BASEBAND_BW_270_kHz,
					FSK_DEVIATION_90_kHz, OUTPUT_POWER_MAX_minus_0dB,
					DATA_RATE_9600_bps);
			dpy_trm_s01__Init();

			// read own ID from DIP switchies
			OWN_ID = DPY_TRM_S01__SWITCH_1_GET_STATE() + (DPY_TRM_S01__SWITCH_2_GET_STATE() << 1) + (DPY_TRM_S01__SWITCH_3_GET_STATE() << 2) + (DPY_TRM_S01__SWITCH_4_GET_STATE() << 3);

			starting = true;
			clk_sync_init();
			cyg_thread_create(TASK_REQUEST_PRIO, // priority
					timed_request, // thread function
					0, // thread parameter
					"Request thread", // thread name
					&timed_request_stack, // stack base addr
					TASK_STACK_SIZE, // stack size
					&timed_request_handle, // thread handle
					&timed_request_obj); // thread object

			cyg_thread_create(TASK_RADIO_RECEIVE_PRIO, // priority
					radio_receive, // thread function
					0, // thread parameter
					"Receive thread", // thread name
					&radio_receive_stack, // stack base addr
					TASK_STACK_SIZE, // stack size
					&radio_receive_handle, // thread handle
					&radio_receive_obj); // thread object


			cyg_thread_create(TASK_ANSWER_PRIO, // priority
					timed_answer, // thread function
					0, // thread parameter
					"Answer thread", // thread name
					&timed_answer_stack, // stack base addr
					TASK_STACK_SIZE, // stack size
					&timed_answer_handle, // thread handle
					&timed_answer_obj); // thread object

			diag_printf("threads initialized\r\n");

			// Creating an interrupt for Timer 1
			cyg_interrupt_create(CYGNUM_HAL_INTERRUPT_TIMER1, // interrupt vector
					CYGNUM_HAL_PRI_HIGH, // priority
					0, // Data
					Timer1_isr, // ISR function
					Timer1_dsr, // DSR function
					&timer1_int_handle, // interrupt handle
					&timer1_int); // interrupt object


			init_Timer1();
			// attach the created interrupt
			cyg_interrupt_attach(timer1_int_handle);
			cyg_interrupt_unmask(CYGNUM_HAL_INTERRUPT_TIMER1);
			// start threads
			cyg_thread_resume(timed_request_handle);
			cyg_thread_resume(radio_receive_handle);
			cyg_thread_resume(timed_answer_handle);

			diag_printf("ISR Init DONE\r\n\r\n");
		}

	/*****************************************************************************************//**
	 * Waits for myflag 2nd bit and broadcasts a synch-request
	 ********************************************************************************************/

void timed_request(cyg_addrword_t index) {

	cyg_flag_value_t flag_value;
	cyg_uint32 current_time;
	cyg_uint32 old_MCR;
	static int message_count = 0;
	struct packet_t packet;

	// Starting the execution
	diag_printf(" radio_transmit started\r\n\r\n");

	while (1) {
		flag_value = cyg_flag_wait(&my_flag, 1, CYG_FLAG_WAITMODE_AND
				| CYG_FLAG_WAITMODE_CLR );
		// Making sure, that cyg_flag_wait()
		// has been returned because of the event
		// and not because of a cyg_thread_release() call


		if (flag_value & 0x01) {
			cyg_mutex_lock(&my_mutex); // locking the mutex
			packet.source = OWN_ID;
			packet.destination = 0;
			packet.message_type = CLK_SYNC_REQUEST;
			packet.Ej = 0;
			packet.Cj = message_count++;
			com_r04_s01__Send_Packet((unsigned char*) &packet, PACKET_SIZE);
			CLK_SYNC_request_sent_time = gettime();
			cyg_mutex_unlock(&my_mutex); // unlocking the mutex

			current_time = gettime();
			// set next request time, can be superseded in clk_sync_handle_request
			setmr0(current_time + 2* MACROTICK*NUMBER_OF_NODES + MACROTICK*OWN_ID);
			// enable match reg IT
			HAL_READ_UINT32(TIMER1_MCR, old_MCR);
			HAL_WRITE_UINT32(TIMER1_MCR, old_MCR | 0x01);
		} else {
			diag_printf("wait on my_flag interrupted\r\n");
		}
	}
}

/*****************************************************************************************//**
 * Receive packages from ether and decides is it an answer
 * or a request and calls proper functions
 ********************************************************************************************/
void radio_receive(cyg_addrword_t index) {

	struct packet_t packet_local;
	unsigned short int length = 100;
	unsigned char receive_result;

	// Starting the execution
	diag_printf(" radio_receive started\r\n\r\n");
	while (1) {
		length = PACKET_SIZE;
		cyg_mutex_lock(&my_mutex); // locking the mutex
		receive_result = com_r04_s01__Receiving_Packet_with_timeout(
				(char*) &packet_local, &length, 100);
		cyg_mutex_unlock(&my_mutex); // unlocking the mutex

		if (receive_result != COM_R04_S01__NOERROR) {
			//diag_printf("hiba %d\r\n", receive_result);
			continue;
		}

		switch (packet_local.message_type) {
		case CLK_SYNC_REQUEST:
			requester = packet_local.source;
			clk_sync_handle_request(&packet_local);
			break;
		case CLK_SYNC_ANSWER:
			if (packet_local.destination == OWN_ID) {
				clk_sync_handle_answer(&packet_local);
			}
			break;
		default:
			diag_printf("hibas csomag");
			break;
		}
	}
}

/*****************************************************************************************//**
 * Calculates clock deviation of mote and broadcast it.
 ********************************************************************************************/
void timed_answer(cyg_addrword_t index) {
	cyg_flag_value_t flag_value;
	struct packet_t l_packet;

	while (1) {
		flag_value = cyg_flag_wait(&my_flag, 2, CYG_FLAG_WAITMODE_AND
				| CYG_FLAG_WAITMODE_CLR );

		if (flag_value & 0x02) {
			l_packet.Cj = gettime();
			if (starting) {
				l_packet.Ej = ERROR_INIT_VALUE;
			} else {
				l_packet.Ej = CLK_SYNC_h + (l_packet.Cj - CLK_SYNC_rho) * abs(
						CLK_SYNC_drift);
			}
			l_packet.message_type = CLK_SYNC_ANSWER;
			l_packet.source = OWN_ID;
			l_packet.destination = requester;

			cyg_mutex_lock(&my_mutex); // locking the mutex
			com_r04_s01__Send_Packet((unsigned char*) &l_packet, PACKET_SIZE);
			cyg_mutex_unlock(&my_mutex); // unlocking the mutex


		} else {
			diag_printf("wait on my_flag interrupted\r\n");
		}
	}
}

/*****************************************************************************************//**
 * Initialize timer 1 for clock.
 * Initialize match register 0 for request timing.
 ********************************************************************************************/
void init_Timer1(void) {
	// Reseting and disabling the Timer1 module
	HAL_WRITE_UINT32(TIMER1_TCR, 0x2);
	// Use the MR0 to create initial request interrupt
	HAL_WRITE_UINT32(TIMER1_MR0, (NUMBER_OF_NODES+ OWN_ID)*MACROTICK);
	// Use the MR2 to create initial periodic interrupt for LED blinking
	HAL_WRITE_UINT32(TIMER1_MR2, BLINK_PERIOD/2);
	// Interrupt with no-reset on MR0 & MR2
	HAL_WRITE_UINT32(TIMER1_MCR, 0x41);
	// Enable Timer 1 and start counting
	HAL_WRITE_UINT32(TIMER1_TCR, 0x01);

	diag_printf("Thread A : Timer 1 initialization Done \r\n\r\n");
}

/*****************************************************************************************//**
 * Initialize variables used by the clock synchronization algorithm
 ********************************************************************************************/
void clk_sync_init(void) {
	CLK_SYNC_h = 600 * OWN_ID;
	CLK_SYNC_rho = 0;
	CLK_SYNC_drift = 0.0;
	CLK_SYNC_request_sent_time = 0;
}

/*****************************************************************************************//**
 * Sets time when next request packet must be sent
 * Sets time when next answer packet must be sent
 * @param packet is a pointer to the received clock synchronization packet
 ********************************************************************************************/
void clk_sync_handle_request(struct packet_t* packet) {
	cyg_uint32 match_reg_value, old_timer;

	HAL_READ_UINT32(TIMER1_TC, old_timer);

	// set request timer
	match_reg_value = old_timer + ((NUMBER_OF_NODES+ OWN_ID - packet->source)
			% NUMBER_OF_NODES) * MACROTICK;
	setmr0(match_reg_value);

	// set response timer
	HAL_READ_UINT32(TIMER1_TC, old_timer);
	match_reg_value = old_timer + OWN_ID * MICROTICK;
	setmr1(match_reg_value);

	// enable match reg IT
	HAL_WRITE_UINT32(TIMER1_MCR, 0x49);
}

/*****************************************************************************************//**
 * Handles the incomming answer messages. Synchronizes local clock if
 * received clock accuracy is better.
 * @param packet is a pointer to the received clock synchronization packet
 ********************************************************************************************/
void clk_sync_handle_answer(struct packet_t* packet) {

	cyg_uint32 current_time; // current time
	cyg_uint32 old_mr; // old request timing match register
	cyg_int32 delta_time; // time since request
	cyg_int32 transmit_delay; // data transmission delay
	cyg_int32 own_error, sender_error; // clock error
	cyg_uint32 new_clock;
	cyg_uint32 clock_modification;

	current_time = gettime();
	delta_time = current_time - CLK_SYNC_request_sent_time;
	transmit_delay = (6+ PACKET_SIZE)*CYGNUM_HAL_CPU_SPEED/9600;//(delta_time - MICROTICK * packet->source) / 2;
	own_error = CLK_SYNC_h + abs(delta_time * CLK_SYNC_drift);
	sender_error = packet->Ej + abs(transmit_delay * (1 + CLK_SYNC_drift));

	// change clock if our clock accuracy is worse or if we are new and the other is not
	if (abs(sender_error) < abs(own_error) || (starting == true && packet->Ej != ERROR_INIT_VALUE)) {
		// if own clock needs to be updated
		new_clock = packet->Cj; //+ transmit_delay * (1 + CLK_SYNC_drift);
		current_time = gettime();
		clock_modification = new_clock - current_time;
		old_mr = getmr0();
		setmr0(clock_modification + old_mr ); // update response timing match register
		old_mr = getmr1();
		setmr1(clock_modification + old_mr ); // update answer timing match register

		HAL_WRITE_UINT32(TIMER1_MR2, (new_clock/(BLINK_PERIOD/2))*(BLINK_PERIOD/2) + (BLINK_PERIOD/2) );
		settime(new_clock); // update clock

		CLK_SYNC_rho = new_clock;
		CLK_SYNC_h = sender_error + (1 + CLK_SYNC_drift) * transmit_delay;
		// exponential average
		CLK_SYNC_drift = 0.9 * CLK_SYNC_drift + 0.1
				* clock_modification/ (NUMBER_OF_NODES*MACROTICK);
		diag_printf("C=%d, E=%d change=%d\r\ndrift = %d\r\n", gettime(), CLK_SYNC_h, clock_modification, (int)CLK_SYNC_drift);
	} else {
		CLK_SYNC_rho = current_time;
		CLK_SYNC_h = own_error/10*9 + CLK_PRECISION*(NUMBER_OF_NODES*MACROTICK/1000000)/10 ;
		CLK_SYNC_drift = 0.9 * CLK_SYNC_drift;
		diag_printf("C=%d, E=%d no change\r\ndrift = %d\r\n", gettime(), CLK_SYNC_h, (int)CLK_SYNC_drift);
	}

	starting = false;
}
